#include <iostream>
#include <fstream>
#include <string>
#include <math.h>
#include <opencv2/opencv.hpp>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "Rune_fiall.h"
using namespace cv;
using namespace std;

#define USE_VIDEO
//#define debug


Rune::Rune()
{
    string file_name = "../rune.xml";
    FileStorage fs(file_name, FileStorage::READ);
    if(!fs.isOpened())
        printf("Cannot open armor param file, please check if the file is exist");
    else
        cout<<"succeseful load rune.xml"<<endl;
    //fs读取
    fs["enemy_color"] >> color;
    fs["Maxarea"] >> Maxarea;
    fs["Minarea"] >> Minarea;
    fs["Maxarea_l"] >> Maxarea_l;
    fs["Minarea_l"] >> Minarea_l;
    fs["img_idx"] >> img_idx;
}

void Rune::RUNE(Mat &src)
{
    double start = static_cast<double>(getTickCount());
    Mat image;
    src.copyTo(image);
    vector<Mat> bgr_channel;
	split(image, bgr_channel);

	if (color == 0){
        binary_color_img =bgr_channel.at(2)-bgr_channel.at(0);
        threshold(binary_color_img,binary_color_img, 50, 255, CV_THRESH_BINARY);//130
    }
	else{
        binary_color_img =bgr_channel.at(0)-bgr_channel.at(2);
        threshold(binary_color_img,binary_color_img, 150, 255, CV_THRESH_BINARY);//130
    }
    
    Mat binary;
    binary_color_img.copyTo(binary);//对binary进行面积筛选，筛选出面积最符合的旋臂轮廓。
    GaussianBlur(binary,binary,Size(3,3),0,0);

    floodFill(binary_color_img,Point(1,1),Scalar(255));
    threshold(binary_color_img,binary_color_img,0,255,THRESH_BINARY_INV);//漫水算法
    Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));
    morphologyEx(binary_color_img, binary_color_img, MORPH_OPEN, element);
    findContours(binary_color_img,contours_color, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
    imshow("binary",binary_color_img);

    vector<vector<Point> > contours_color_l;
    vector<vector<Point> > contours_color_finall;
    findContours(binary,contours_color_l, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);//旋臂

#ifdef debug
    imshow("binary_color_img",binary_color_img);
    imshow("binary",binary);
#endif
    vector<vector<Point> > contours_r;
    for(int i=0;i<contours_color_l.size();i++)//旋臂筛选
    {
        double Cont = fabs(contourArea(contours_color_l[i],true));
        if(Cont < Maxarea_l && Cont > Minarea_l){
            contours_color_finall.push_back(contours_color_l[i]);
        }
        if(Cont< 500 && Cont> 200){
            contours_r.push_back(contours_color_l[i]);
        }
    }
    if(contours_color_finall.empty()){
        return;
    }

    vector<vector<Point> >contours_ploy(contours_color.size());
    vector<RotatedRect> RotatedRect_ploy;
    vector<RotatedRect> RotatedRect_svms;
    RotatedRect predict_rect;
    // Point2f predict_rect_points[4],predict_rect_points_finall[4];
    float min,max;

    for(int i=0;i<contours_color.size();i++)
    {
        double Cont = fabs(contourArea(contours_color[i],true));
        approxPolyDP(contours_color[i], contours_ploy[i], 5, true);//最小包围矩形
        RotatedRect temp1 = minAreaRect(contours_ploy[i]);
        if(temp1.size.width > temp1.size.height){
            min = temp1.size.height;
            max = temp1.size.width;
        }
        else{
            max = temp1.size.height;
            min = temp1.size.width;
        }
        if(max/min >1.5 && max/min< 2.2 && Cont < Maxarea && Cont > Minarea){
            RotatedRect_ploy.push_back(temp1);
        }
    }

    Point2f pot_cen,cen_R;
    Point2f pot[4],pots[4];
    float angle;
    for (int i = 0; i< RotatedRect_ploy.size(); i++)
    {
        RotatedRect_ploy[i].points(pot);
        if(pointPolygonTest(contours_color_finall[0],RotatedRect_ploy[i].center,false) == 1)//判断那个装甲板在旋臂里
        {
            predict_rect = RotatedRect_ploy[i];
            angle = RotatedRect_ploy[i].angle;
            pot_cen = RotatedRect_ploy[i].center;
            for(int j=0; j<4; j++)
            {
                line(image, pot[j], pot[(j+1)%4], Scalar(0,0,255),2);
            }
            break;
        }
    }

// 圆的方程预测

    if(contours_r.empty()){
        cen_R =his_cen_point;
    }
    else{
        cen_R = get_center_point(contours_r[0]);
        his_cen_point = cen_R;
    }
    // cout<<cen_R<<endl;
    // cout<<RotatedRect_ploy[0].center<<endl;
    // float L_R = distance_hanshu(pot_cen,cen_R); 

    for(int i = 0;i < RotatedRect_ploy.size();i++)
    {
        RotatedRect_ploy[i].points(pot);
        Point2f center_1;
        center_1.x = (cen_R.x + RotatedRect_ploy[i].center.x)/2;
        center_1.y = (cen_R.y + RotatedRect_ploy[i].center.y)/2;
        Size2f size_svm;
        size_svm.height = 120;
        size_svm.width = 55;
        float angle_svm;

        if((cen_R.x <= RotatedRect_ploy[i].center.x && cen_R.y >= RotatedRect_ploy[i].center.y) ||(cen_R.x >= RotatedRect_ploy[i].center.x && cen_R.y <= RotatedRect_ploy[i].center.y)){
            angle_svm =RotatedRect_ploy[i].angle+90;
            RotatedRect RotatedRect_svm(center_1,size_svm,angle_svm);
            RotatedRect_svm.points(pots);
            RotatedRect_svms.push_back(RotatedRect_svm);
        }
        else{
            angle_svm = RotatedRect_ploy[i].angle;
            RotatedRect RotatedRect_svm(center_1,size_svm,angle_svm);
            RotatedRect_svm.points(pots);
            RotatedRect_svms.push_back(RotatedRect_svm);
        }
        Mat img;
        Mat rot_mat = cv::getRotationMatrix2D(RotatedRect_svms[i].center,RotatedRect_svms[i].angle,1); 
        Point2f center = RotatedRect_svms[i].center;
        warpAffine(binary, img, rot_mat, img.size(), INTER_LINEAR, BORDER_CONSTANT); 


        Rect target = cv::Rect(center.x - (RotatedRect_svms[i].size.width / 2), 
                                center.y - (RotatedRect_svms[i].size.height/2), 
                                 RotatedRect_svms[i].size.width, RotatedRect_svms[i].size.height);
        Mat armor_roi = img(target);
        
        // char str[100];
        // sprintf(str,"picture/armor_%d.jpg", img_idx++);
        // imwrite(str,armor_roi);

    //    float svl;
    //     svl = rune_svm(armor_roi);
    //     cout<<"svl:"<<svl<<endl;
    //     if(svl > -1){
    //        for(int j=0; j<4; j++)
    //          line(image, pots[j], pots[(j+1)%4], Scalar(0,0,255),2);
    //        for(int j=0; j<4; j++)
    //          line(image, pot[j], pot[(j+1)%4], Scalar(0,255,0),2);
            //  }

    RotatedRect_ploy.clear();

    // w0 = asin((pot_cen.x-cen_R.x)/L_R);
    // float w1 = 0.0349*5*3;//2度对应的弧度0.0349
    // if((pot_cen.x-his.x)>0 &&(pot_cen.y-his.y)<0)//判断顺时针还是逆时针
    //     colorwise = 1;
    // else
    //     colorwise = 0;
    // if(colorwise == 0){//顺时
    //     if(pot_cen.y < cen_R.y){
    //         predict_point.x = sin(w0+w1)*L_R+cen_R.x;
    //         predict_point.y = -cos(w0+w1)*L_R+cen_R.y;
    //     }
    //     else{
    //         predict_point.x = sin(w0-w1)*L_R+cen_R.x;
    //         predict_point.y = cos(w0-w1)*L_R+cen_R.y;
    //     }
    // }
    // else{//逆时针
    //     if(pot_cen.y < cen_R.y){
    //         predict_point.x = sin(w0-w1)*L_R+cen_R.x;
    //         predict_point.y = -cos(w0-w1)*L_R+cen_R.y;
    //     }
    //     else{
    //         predict_point.x = sin(w0+w1)*L_R+cen_R.x;
    //         predict_point.y = cos(w0+w1)*L_R+cen_R.y;
    //     }
    // }

    // his = pot_cen;

    // predict_rect.center = predict_point;
    // predict_rect.angle = -(fabs(angle) - w1*180/3.1415926);
    // predict_rect.points(predict_rect_points_finall);


//**********************
    imshow("img",image);
    double time = ((double)getTickCount() - start) / getTickFrequency();
    //cout << time << "秒" << endl;

    contours_r.clear();
    contours_color.clear();
    contours_color_l.clear();
    contours_color_finall.clear();

    // vector<Point2f> result;
    // for(int i=0;i<4;i++){
    //     result.push_back(predict_rect_points_finall[i]);
    // } 
    
    char key = (char)waitKey(0);
    if (key == 27)
        return;
    }
}
int main()
{
    Rune rune;
    VideoCapture cap;

#ifdef USE_VIDEO 
    if(color == 0){
        cap.open("../../video/armor_20190710_203521.mp4");//14
        cout<<"successful load red image"<<endl;
    }
    else{
        cap.open("../../video/b1.mp4");
        cout<<"successful load blue image"<<endl;
    }
#else
    cap.open(1);
#endif

while(1)
    {
        cap >> src;
        rune.RUNE(src);
    }
    return 0;
}
